#include "golink.h"
#include "hal_rs485.h"
#include <string.h>
#include "hal_tty.h"

#include "FreeRTOS.h"
#include "task.h"

#define X25_INIT_CRC    0xFFFF

static uint16_t crc_tab16[256];
static golink_status_t status_;
static uint8_t crc_tab16_init = 0;

void crc_table_init() {

    if(!crc_tab16_init) {
        int i, j;
        uint16_t crc_init, c;

        for(i = 0; i < 256; i++) {
            crc_init = 0;
            c = i;
            for(j = 0; j < 8; j++) {
                if((crc_init ^ c) & 0x0001) {
                    crc_init = ( crc_init >> 1 ) ^ CRC_POLY_16;
                } else {
                    crc_init = crc_init >> 1;
                }
                c = c >> 1;
            }
            crc_tab16[i] = crc_init;
        }
        crc_tab16_init = 1;
    }
}

uint16_t golink_crc16(const uint8_t *input_str, int num_bytes) {

    uint16_t crc, tmp, short_c;
    const uint8_t *ptr;
    int index;

    crc_table_init();

    crc = CRC_START_16;
    ptr = input_str;

    if(ptr != NULL) {
        for (index = 0; index < num_bytes; index++) {

            short_c = 0x00ff & (uint16_t) *ptr;
            tmp = crc ^ short_c;
            crc = (crc >> 8) ^ crc_tab16[tmp & 0xff];
            ptr++;
        }
    }
    return crc;
}

uint16_t golink_msg_crc16(const golink_msg_t *msg, int num_bytes) {

	uint16_t crc, tmp, short_c;
    const uint8_t *ptr = NULL;
    int index;

    crc_table_init();
    crc = CRC_START_16;
    short_c = msg->msg_id & 0x00ff;

    tmp = crc ^ short_c;
    crc = (crc >> 8) ^ crc_tab16[tmp & 0xff];
    ptr = msg->payload;

    if(ptr != NULL) {
        for (index = 0; index < num_bytes - 1; index++) {
            short_c = 0x00ff & (uint16_t) *ptr;
            tmp = crc ^ short_c;
            crc = (crc >> 8) ^ crc_tab16[tmp & 0xff];
            ptr++;
        }
    }
    return crc;
}

void init_golink_status() {
    status_.states = GOLINK_STATE_IDLE;
}

int golink_send_message(uint8_t msg_id,uint8_t msg_len,const char *data) {
    uint8_t buf[16];
    int index = 0;
    buf[index++] = GOLINK_STX1;
    buf[index++] = GOLINK_STX2;
    buf[index++] = status_.tx_seq++;
    buf[index++] = msg_len + 1;
    buf[index++] = msg_id;
    memcpy(&buf[index],data,msg_len);
    index += msg_len;
    uint16_t checksum = golink_crc16(buf + 4,msg_len + 1);

    buf[index++] = (uint8_t)(checksum & 0xFF);
    buf[index++] = (uint8_t)((checksum >> 8) & 0xFF);

    g_rs485_.write(buf,index);
    return 0;
}

int golink_parse_buf(uint8_t c,golink_msg_t *msg) {
    int msg_received = GOLINK_FRAMING_INCOMPLETE;

    switch (status_.states) {
        case GOLINK_STATE_UNINIT:
        case GOLINK_STATE_IDLE:
            if(c == GOLINK_STX1) {
                status_.states = GOLINK_STATE_GOT_STX1;
            }
            break;
        case GOLINK_STATE_GOT_STX1:
            if(c == GOLINK_STX2) {
                status_.states = GOLINK_STATE_GOT_STX2;
            } else 
                status_.states = GOLINK_STATE_IDLE;
            break;
        case GOLINK_STATE_GOT_STX2:
            status_.states = GOLINK_STATE_GOT_SEQ;
            msg->seq = c;
            break;
        case GOLINK_STATE_GOT_SEQ:
            msg->msg_len = c;
            status_.states = GOLINK_STATE_GOT_LEN;
            status_.msg_idx = 0;
            break;
        case GOLINK_STATE_GOT_LEN:
            msg->msg_id = c;
            status_.states = GOLINK_STATE_GOT_MSGID;
            break;
        case GOLINK_STATE_GOT_MSGID:
            msg->payload[status_.msg_idx++] = c;
            if(status_.msg_idx >= (msg->msg_len - 1)) {
                status_.states = GOLINK_STATE_GOT_PAYLOAD;
				msg->msg_crc = golink_msg_crc16(msg,msg->msg_len);
            }
            break;
        case GOLINK_STATE_GOT_PAYLOAD:
            if(c == (msg->msg_crc & 0xFF)) {
                status_.states = GOLINK_STATE_GOT_LSB;
            } else {
                status_.states = GOLINK_STATE_IDLE;
                msg_received = GOLINK_FRAMING_BAD_CRC;
                status_.error_packet++;
                status_.rx_seq++;
            }
            break;
        case GOLINK_STATE_GOT_LSB:
            if(c == ((msg->msg_crc >> 8) & 0xFF)) {
                msg_received = GOLINK_FRAMING_OK;
            } else {
                msg_received = GOLINK_FRAMING_BAD_CRC;
				g_tty_.printf("msg_ crc error!!\r\n");
                status_.error_packet++;
            }
            status_.rx_seq++;
            status_.states = GOLINK_STATE_IDLE;
            break;
        default:
            break;
    }

    return msg_received;
}
